/*
*
* File Name          :  Hrtf.c
* Description        :  HRTF Functions
*/

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include "Hrtf.h"
#include "HrtfArray.h"
#include "circle_api.h"

int HRTF_Init() {
  DSP_Init(DSP_FFT_SAMPLE_LENGTH);
  return 0;
}

void HRTF_ComplexArrayMultiply(FPComplex *output, FPComplex *input1, FPComplex *input2, int length) {
  int i;
    
  for( i = 0 ; i < length ; i++ ) {
    output[i][0] = input1[i][0]*input2[i][0] - input1[i][1]*input2[i][1];
    output[i][1] = input1[i][0]*input2[i][1] + input1[i][1]*input2[i][0];
  }
}

void HRTF_ComplexArrayAdd(FPComplex *output, FPComplex *input1, FPComplex *input2, int length) {
  int i;
  
  for( i = 0 ; i < length ; i++ ) {
    output[i][0] = input1[i][0]+input2[i][0];
    output[i][1] = input1[i][1]+input2[i][1];
  }
}

void HRTF_ArrayMultiply(FPComplex *output, FPComplex *input1, int16_t *input2, int length) {
  int i;
  
  for( i = 0 ; i < length ; i++ ) {
    output[i][0] = input1[i][0]*input2[i];
    output[i][1] = input1[i][1]*input2[i];
  }
}

void HRTF_ReadDb(HRTF_StereoSignal *hrtfDb, float elevation, float azimuth, int sampleLength) {
  int i, flip, hrtfElevationIndex, hrtfAzimuthIndex, hrtfArrayIndex;
  const int16_t *tmpArray, *leftArray, *rightArray;
  char str1[16];
  
  if(azimuth<0)
    flip = 1;
  else
    flip = 0;
  if(elevation<HRTF_ELEVATION_MIN)
    elevation = HRTF_ELEVATION_MIN;
  else if(elevation>HRTF_ELEVATION_MAX)
    elevation = HRTF_ELEVATION_MAX;
  
  hrtfElevationIndex = (int) round((elevation-HRTF_ELEVATION_MIN)/10.0);
  azimuth = abs(azimuth);  
  if(azimuth>HrtfArrayAzimuthMax[hrtfElevationIndex])
      azimuth = HrtfArrayAzimuthMax[hrtfElevationIndex];
  
  hrtfAzimuthIndex = (int) round(azimuth*(HrtfArrayOffset[hrtfElevationIndex+1]-HrtfArrayOffset[hrtfElevationIndex]-1)/HrtfArrayAzimuthMax[hrtfElevationIndex]);
  hrtfArrayIndex = HrtfArrayOffset[hrtfElevationIndex]+hrtfAzimuthIndex;
  leftArray = &HrtfArray[hrtfArrayIndex][0][0];
  rightArray = &HrtfArray[hrtfArrayIndex][1][0];
  if(flip) {
    tmpArray = leftArray;
    leftArray = rightArray;
    rightArray = tmpArray;
  }
  
  for( i = 0 ; i < HRTF_DB_LENGTH ; i++ ) {
    hrtfDb->left[i][0] = leftArray[i];
    hrtfDb->left[i][1] = 0;
    hrtfDb->right[i][0] = rightArray[i];
    hrtfDb->right[i][1] = 0;
  }
  
  for( i = HRTF_DB_LENGTH ; i < sampleLength ; i++ ) {
    hrtfDb->left[i][0] = 0.0;
    hrtfDb->left[i][1] = 0.0;
    hrtfDb->right[i][0] = 0.0;
    hrtfDb->right[i][1] = 0.0;
  }

  UTIL_int2str( &str1[0], hrtfArrayIndex, 3, 1 );
  DRAW_DisplayString( 80, 90, str1, 4);
}

void HRTF_SoundPosition(HRTF_StereoSignal *stereoSignal, FPComplex *signal, int length, float elevation, float azimuth) {
  FPComplex fp1[DSP_FFT_SAMPLE_LENGTH], fp2[DSP_FFT_SAMPLE_LENGTH], fp3[DSP_FFT_SAMPLE_LENGTH];
  
  HRTF_StereoSignal *hrtfDb;

  hrtfDb = (HRTF_StereoSignal *)stereoSignal;
  HRTF_ReadDb(hrtfDb, elevation, azimuth, length);
  DSP_Fft(fp1, signal);

  DSP_Fft(fp2, hrtfDb->left);
  HRTF_ComplexArrayMultiply(fp3, fp2, fp1, length);
  DSP_Ifft(stereoSignal->left, fp3);

  DSP_Fft(fp2, hrtfDb->right);
  HRTF_ComplexArrayMultiply(fp3, fp2, fp1, length);
  DSP_Ifft(stereoSignal->right, fp3);
}